package simpleGeom;



import java.util.ArrayList;
import java.util.Vector;

import processing.core.PVector;


public class Plane {
Vec3D normal;//normal;
float d0=0;//distanceToOrigin;
//hessian normal form
public Plane( HasPos3D p,HasPos3D normal){
	//this.p=new Vec3D(p);
	this.normal=new Vec3D(normal);
	this.normal.normalize();
	d0=-this.normal.dot(p);
	
}
public Plane( HasPos3D p0,HasPos3D p1,HasPos3D p2 ){
	//this.p=new Vec3D(p);
	this.normal= Vec3D.getNormal(p0, p1, p2);
	this.normal.normalize();
	d0=-this.normal.dot(p0);
}
public float getDistanceSigned(PVector v){
	return getDistanceSigned(v.x, v.y, v.z);
}
public float getDistanceSigned(HasPos3D v){
	return getDistanceSigned(v.getX(), v.getY(), v.getZ());
}
public float getDistanceSigned(float x, float y, float z){
	return normal.dot(x, y, z)+d0;
}

public Vec3D intersectWithRay(HasPos3D v0,HasPos3D v1){
	return intersectWithLine(v0.getX(),v0.getY(),v0.getZ(),v1.getX(),v1.getY(),v1.getZ());
}

public Vec3D getPlanePoint(Vec3D v){
	Vec3D normal=getNormal();
	return intersectWithLine(v.getX(),v.getY(),v.getZ(),v.getX()+normal.getX(),v.getY()+normal.getY(),v.getZ()+normal.getZ());
}
public Vec3D intersectWithLine(float ax,float ay,float az,float bx,float by,float bz){
	float d0=getDistanceSigned(ax,ay,az);
	float d1=getDistanceSigned(bx,by,bz);
	Vec3D v=new Vec3D(bx-ax,by-ay,bz-az);
	v.mult(d0/(d0-d1));
	v.setPosition(v.x+ax,v.y+ay,v.z+az);
	//Q = P0 +(d0/(d0 −d1))(P1 −P0)
	return v;
}

public Mesh mirror(Mesh mesh){
	Mesh copyMesh=mesh.getCopy();
	for (int i=0;i<copyMesh.getNNodes();i++){
		MeshNode n=copyMesh.getNode(i);
		Vec3D planeP=getPlanePoint(n);
		Vec3D v=Vec3D.sub(planeP, n);
		v.mult(2);
		n.add(v);
	}
	return copyMesh;
}


public Mesh cutMesh(Mesh mesh){
	Mesh copyMesh=mesh.getCopyNodes();
	ArrayList<MeshFace>cutFaces=new ArrayList<MeshFace>();
	ArrayList<MeshEdge>cutEdges=new ArrayList<MeshEdge>();
	for (int i = 0; i < mesh.getNNodes(); i++) {
		MeshNode n = mesh.getNode(i);
		n.comment=0;
	}
	for (int i = 0; i < mesh.getNNodes(); i++) {
		MeshNode n = mesh.getNode(i);
		float d1 = getDistanceSigned(n.getX(), n.getY(), n.getZ());
		if (d1 >= 0 ) {
			n.comment=1;
		}
	}
	for (int i = 0; i < mesh.getNFaces(); i++) {
		MeshFace f = mesh.getFace(i);
		int nInside=0;
		for (int ii=0;ii<f.getNNodes();ii++){
			MeshNode n=f.getNode(ii);
			if (n.comment==1){
				nInside++;
			}
		}
		if (nInside>2){
			Vector<MeshNode>newNodes=new Vector<MeshNode>();
			for (int ii=0;ii<f.getNNodes();ii++){
				MeshNode cNode=f.getNode(ii);
				MeshNode newNode=copyMesh.getNode(cNode.getId());
				newNodes.add(newNode);
			}
			copyMesh.addFace(newNodes);
		}
		else if (nInside>0){
			
			for (int ii=0;ii<f.getNNodes();ii++){
				MeshNode n1=f.getNode(ii);
				MeshNode n2=f.getNode(ii+1);
				MeshNode n3=f.getNode(ii+2);
				if (n1.comment==0&&n2.comment==1&&n3.comment==0){
					Vec3D newV1=intersectWithRay(n1,n2);
					Vec3D newV2=intersectWithRay(n2,n3);
					
					Vector<MeshNode>splitNodes=new Vector<MeshNode>();
					MeshNode newNode1=copyMesh.addNode(newV1.getX(), newV1.getY(), newV1.getZ());
					MeshNode newNode2=copyMesh.addNode(newV2.getX(), newV2.getY(), newV2.getZ());
					
					splitNodes.add(newNode1);
					splitNodes.add(copyMesh.getNode(n2.getId()));
					splitNodes.add(newNode2);
					copyMesh.addFace(splitNodes);
					break;
				}
				else if (n1.comment==0&&n2.comment==1&&n3.comment==1){
					Vec3D newV1=intersectWithRay(n1,n2);
					Vec3D newV2=intersectWithRay(n3,n1);
					
					
					MeshNode newNode1=copyMesh.addNode(newV1.getX(), newV1.getY(), newV1.getZ());
					MeshNode newNode2=copyMesh.addNode(newV2.getX(), newV2.getY(), newV2.getZ());
					Vector<MeshNode>splitNodes1=new Vector<MeshNode>();
					splitNodes1.add(newNode1);
					splitNodes1.add(copyMesh.getNode(n2.getId()));
					splitNodes1.add(copyMesh.getNode(n3.getId()));
					copyMesh.addFace(splitNodes1);
					
					Vector<MeshNode>splitNodes2=new Vector<MeshNode>();
					splitNodes2.add(copyMesh.getNode(n3.getId()));
					splitNodes2.add(newNode2);
					splitNodes2.add(newNode1);
					copyMesh.addFace(splitNodes2);
					break;
				}
			}
		}
	}
	copyMesh.removeUnusedNodes();
	return copyMesh;
}

public void getIntersections(HasPos3D p1,HasPos3D p2,HasPos3D p3){
	
}

public Vec3D getNormal() {
	return normal;
}
public void setNormal(Vec3D normal) {
	this.normal = normal;
}
final static float tol=0.0000001f;
public static HasPos3D[][] intersectTriPlane(HasPos3D a1, HasPos3D a2,
	HasPos3D a3, HasPos3D b1, HasPos3D b2, HasPos3D b3) {
	return intersectTriPlane(a1,a2,a3,new Plane(b1, b2, b3));
}
public static HasPos3D[][] intersectTriPlane(HasPos3D a1, HasPos3D a2,
		HasPos3D a3, Plane plane) {
		HasPos3D[] tri = new HasPos3D[] { a1, a2, a3 };
		float[] dA = new float[3];
		for (int i = 0; i < tri.length; i++) {
			float dist = plane.getDistanceSigned(tri[i]);
			dA[i]=dist;
		}
		HasPos3D[] iPts=new HasPos3D[2];
		int[] ids=new int[2];
		
		int iID=0;
		for (int i = 0; i < 3; i++) {
			int i2 = (i + 1) % 3;
			float dist1 = dA[i];
			float dist2 = dA[i2];
			if (Math.abs(dist1) < tol) {
				if (Math.abs(dist2) > tol) {
					iPts[iID]=tri[i];
					ids[iID]=i;
					iID++;
				}
			} else {
				if ((dist1 < -tol && dist2 > tol)
						|| (dist1 > tol && dist2 < -tol)) {
					iPts[iID]=plane.intersectWithRay(tri[i], tri[i2]);
					ids[iID]=i;
					iID++;
				}
			}
		}
		if (iID != 2)return null;	
		HasPos3D int1 = iPts[0];
		int id1=ids[0];
		HasPos3D int2 = iPts[1];
		int id2=ids[1];
		if (int1==tri[id1]) {
			HasPos3D[][] trias=new HasPos3D[2][3];
			trias[0]=new HasPos3D[]{int1, tri[(id1 + 1) % 3], int2};
			trias[1]=new HasPos3D[]{int1, int2, tri[(id1 + 2) % 3]};
			return trias;
		} else if (int2==tri[id2]) {
			HasPos3D[][] trias=new HasPos3D[2][3];
			trias[0]=new HasPos3D[]{int2, tri[(id2 + 1) % 3], int1};
			trias[1]=new HasPos3D[]{int2, int1, tri[(id2 + 2) % 3]};
			return trias;
		} else {
			HasPos3D[][] trias=new HasPos3D[3][3];
			trias[0]=new HasPos3D[]{int1, int2, tri[(id2 + 1) % 3]};
			trias[1]=new HasPos3D[]{int2, int1, tri[(id1 + 1) % 3]};
			if (id1 + 1 == id2) {
				trias[2]=new HasPos3D[]{int1, tri[(id2 + 1) % 3],
						tri[(id2 + 2) % 3]};
			} else {
				trias[2]=new HasPos3D[]{tri[(id1 + 1) % 3],
						tri[(id1 + 2) % 3], int2};
			}
			return trias;
		}
	}
public static HasPos3D[][] cropTriPlane(HasPos3D a1, HasPos3D a2,
		HasPos3D a3, Plane plane) {
		HasPos3D[] tri = new HasPos3D[] { a1, a2, a3 };
		float[] dA = new float[3];
		for (int i = 0; i < tri.length; i++) {
			float dist = plane.getDistanceSigned(tri[i]);
			dA[i]=dist;
		}
		HasPos3D[] iPts=new HasPos3D[2];
		int[] ids=new int[2];
		
		int iID=0;
		Vector<HasPos3D>rightPts=new Vector<HasPos3D>(4);
		Vector<HasPos3D>leftPts=new Vector<HasPos3D>(4);
		for (int i = 0; i < 3; i++) {
			int i2 = (i + 1) % 3;
			float dist1 = dA[i];
			float dist2 = dA[i2];
				if (dist1 <=0){
					leftPts.add(tri[i]);
				}
				if (dist1 >=0){
					rightPts.add(tri[i]);
				}
				if (dist1<-tol&&dist2>tol||dist1>tol&&dist2<-tol){
					Vec3D v=plane.intersectWithRay(tri[i], tri[i2]);
					leftPts.add(v);
					rightPts.add(v);
				}
		}
		HasPos3D[] lPts=new HasPos3D[leftPts.size()];
		for (int i=0;i<leftPts.size();i++){
			lPts[i]=leftPts.get(i);
		}
		HasPos3D[] rPts=new HasPos3D[rightPts.size()];
		for (int i=0;i<rightPts.size();i++){
			rPts[i]=rightPts.get(i);
		}
		HasPos3D[][] pts=new HasPos3D[2][];
		pts[0]= lPts;
		pts[1]= rPts;
		return pts;
	}
}

